\section{Kinematics}
This section answers questions related to the kinematics of the robot.

\subsection{Forward Kinematics}
Figure \ref{fig:fwdder} shows the derivations that were used to obtain the forward kinematics of the robot. The forward kinematics were completed by using geometric methods. 


\begin{figure}
  \begin{center}
    \epsfig{file=fwdkin.eps, bb=50 50  505 505, width=12cm}
  \end{center}
  \caption{Forward Kinematics Derivation}
  \label{fig:fwdder}
\end{figure}

Thus, the forward kinematics to obtain (x,y,z) is as follows:
  \[ \left( \begin{array}{c}
    x \\
    y \\
    z \end{array} \right)
  =
  \left( \begin{array}{c}
    D \tan(q_1 + 90^\circ) \\
    -D \\
    h + l_1 \sin(q_3) + (\frac{D}{\sin(q_1)} + l_1\cos(q_3))\tan(180^\circ - q_2)+ \frac{a}{\cos(180^\circ - q_2)} \end{array} \right)\] 


\subsection{Inverse Kinematics}
To find $q_1$: 
\begin{equation}
  x = D \tan(q_1 + 90^\circ)
\end{equation}

\begin{equation}
  \label{eq:q1_inv}
  q_1=\tan^{-1} (\frac{x}{D}-90^\circ)
\end{equation}

To Find $q_2$:
\begin{equation}
  z=h + l_1 \sin(q_3) + (\frac{D}{\sin(q_1)} + l_1\cos(q_3))\tan(180^\circ - q_2)+ \frac{a}{\cos(180^\circ - q_2)}
\end{equation}

\begin{equation}
  z\cos(q_2)=(h + l_1 \sin(q_3))\cos(q_2) - (\frac{D}{\sin(q_1)} + l_1\cos(q_3))\sin(q_2)+ a
\end{equation}

\begin{equation}
  \label{eq:z_fwd_kin}
  a = (z - h - l_1 \sin(q_3)) \cos(q_2) + (\frac{D}{\sin(q_1)} + l_1\cos(q_3))\sin(q_2)
\end{equation}

Therefore, let
\begin{equation}
  A = z - h - l_1 \sin(q_3)
\end{equation}
\begin{equation}
  B = \frac{D}{\sin(q_1)} + l_1\cos(q_3)
\end{equation}
\begin{equation}
  C = a
\end{equation}

Changing formula (\ref{eq:z_fwd_kin}) to polar form uses the following formula
\begin{equation}
  C = R \sin(q_2+g)
\end{equation}
where
\begin{equation}
  R = \sqrt{A^2+B^2}
\end{equation}
\begin{equation}
  g = \tan^{-1} \left(\frac{A}{B}\right)
\end{equation}

Therefore
\begin{equation}
  \label{eq:q2_inv}
  q_2 = \sin^{-1}\left(\frac{C}{R}\right)-g
\end{equation}


\subsection{Hitting the Target}
To find the exact joint angles, the equations (\ref{eq:q1_inv}) and (\ref{eq:q2_inv}) from the inverse kinematics were used. Setting $q_3 = \pi/4$ and the target location: (x,y,z)=(0.50,-0.793,0.31) m resolves the following joing angles
\begin{equation}
  q_1 = -1.0082
\end{equation}
\begin{equation}
  q_2 = 2.7607
\end{equation}
\begin{equation}
  q_3 = 0.7854
\end{equation}
After trying to achieve the desired target point the first time, it appeared that the laser point did not hit the target point exactly. The laser point ended up being about 2 cm off towards the bottom right corner. At this point, the home position was recalibrated once again. A second attempt followed and the laser point now appeared 1 cm off directly above the target. It was observed that the calibration of the home position was critical for the laser point to hit the target exactly. 

\subsection{Maintain the Spot Position}
After repeating the calculations using (\ref{eq:q1_inv}) and (\ref{eq:q2_inv}) from the inverse kinematic equations for each value of $q_3$, the following values were found as listed in Table \ref{table:jointval}.

\begin{table}[h!b!p!]
\label{table:jointval}
\caption{Joint Angles}
\begin{center}
\begin{tabular}{| l | l | l | l | l | l |}
\hline
Time&Q1&Q2&Q3&Q2'&Q3'\\
\hline
1&-1.0082&2.7580&1.5708&-0.3836&0.0000\\
2&-1.0082&2.7492&1.4835&-0.3924&-0.0873\\
3&-1.0082&2.7404&1.3693&-0.4012&-0.2015\\
4&-1.0082&2.7372&1.3090&-0.4044&-0.2618\\
5&-1.0082&2.7343&1.2217&-0.4072&-0.3491\\
6&-1.0082&2.7339&1.1345&-0.4077&-0.4363\\
7&-1.0082&2.7361&1.0472&-0.4055&-0.5236\\
8&-1.0082&2.7427&0.9399&-0.3989&-0.6309\\
9&-1.0082&2.7493&0.8727&-0.3923&-0.6981\\
10&-1.0082&2.7607&0.7854&-0.3809&-0.7854\\
\hline
\end{tabular}
\end{center}
\end{table}
Figure \ref{fig:linear_joints} shows the resulting joint angle outputs of using the linear interpolation captured by the robot software. Figure \ref{fig:smooth_joints} shows the resulting joint angle output of using the smooth (cubic spline) interpolation captured by the robot software. Figure \ref{fig:linear_torque} and \ref{fig:smooth_torque} show the torque outputs for the linear and smooth interpolation respectively. 

\begin{figure}
  \begin{center}
    \includegraphics[width=1.0\linewidth]{linear_jangles.eps}
  \end{center}
  \caption{Joint Angle Plots using Linear Interpolation}
  \label{fig:linear_joints}
\end{figure}
\begin{figure}
  \begin{center}
    \includegraphics[width=1.0\linewidth]{cubic_jangles.eps}
  \end{center}
  \caption{Joint Angle Plots Using Smooth Interpolation}
  \label{fig:smooth_joints}
\end{figure}

\begin{figure}
  \begin{center}
    \includegraphics[width=1.0\linewidth]{linear_torques.eps}
  \end{center}
  \caption{Torque Output Plots using Linear Interpolation}
  \label{fig:linear_torque}
\end{figure}
\begin{figure}
  \begin{center}
    \includegraphics[width=1.0\linewidth]{cubic_torques.eps}
  \end{center}
  \caption{Torque Output Plots Using Smooth Interpolation}
  \label{fig:smooth_torque}
\end{figure}


The results of the linear interpolation show the movements to be very rigid with sharp points of transition especially at the start and end of the test. The results of the smooth interpolation show the movements to be very smooth and allows the robot to momentarily pause at each desired point. When comparing the torque outputs of the linear interpolation versus the smooth interpolation, the linear interpolation appeared to have smaller peaks of required torque to move the manipulator from one position to another. This is visible especially when comparing the first joint output at the 1 second mark. The linear interpolation peaks above 1 N-m while the smooth interpolation output manages to stay just above 0.5 N-m. 

By visual observations, although the laser does not hit its target, it does not move from the spot when q3 rotates from 0 to $\pi$/4 rad. This shows that the accuracy of the robot is truly dependent on the initial conditions when setting up the home position. Because the plots do not show the joint angles to be jittery and show rather a very smooth transition between one value to another at each second, it can be said that the precision of these joint angle sensors are quite high since we cannot visually see the encoder's qauntization of the recorded values. 

